#include <stdio.h>
#include <string.h>
#include <math.h>
#include "pico/stdlib.h"
#include "pico/bootrom.h"
#include "hardware/spi.h"
#include "hardware/adc.h"
#include "hardware/irq.h"
#include "hardware/pwm.h"
#include "lsm6ds3tr-c_reg.h"
#include "led8x8.h"

#ifndef M_PIf
#define M_PIf 3.14159265358979323846f /* pi */
#endif

#define SPI_PORT spi0
#define PIN_MISO 16
#define PIN_CS 17
#define PIN_SCK 18
#define PIN_MOSI 19

#define BUZ 6
#define P5V_SENSE 7
#define PWR_KEY 11
#define VBAT_EN 21
#define PWR_EN 22
#define VBAT_SENSE 26
#define VBAT_SENSE_ADC_CH 0

#define COL0 0
#define COL1 27
#define COL2 25
#define COL3 2
#define COL4 24
#define COL5 1
#define COL6 3
#define COL7 4
const uint8_t COL[8] = {COL0, COL1, COL2, COL3, COL4, COL5, COL6, COL7};

#define ROWA 10
#define ROWB 9
#define ROWC 12
#define ROWD 8
#define ROWE 29
#define ROWF 13
#define ROWG 28
#define ROWH 23
const uint8_t ROW[8] = {ROWA, ROWB, ROWC, ROWD, ROWE, ROWF, ROWG, ROWH};
/* 8x8 LED 控制代码 -------------------------------------------------------------*/
// 125Mz / 8 / 2 / 3906 = 2000Hz
#define LED_MAX 3906
uint16_t display_buffer[8][8] = {0};
/**
 * @brief 当PWM周期循环结束时调用的函数，用于更新LCD显示的每一行。
 * 这个函数不接受参数，也不返回任何值。
 * 它通过静态变量来循环选择行，并对这一行进行更新。
 * 具体步骤包括：清除所有行的GPIO状态，更新当前行的GPIO电平以显示新的图像数据，
 * 然后设置下一个要更新的行。
 */
void __not_in_flash_func(on_pwm_wrap)(void)
{
  // 当前要更新的行号
  static uint8_t row_now = 0;
  // 清除所有行的GPIO状态
  gpio_clr_mask((1 << ROWA) | (1 << ROWB) | (1 << ROWC) | (1 << ROWD) |
                (1 << ROWE) | (1 << ROWF) | (1 << ROWG) | (1 << ROWH));
  // 根据当前行号选择对应的行GPIO
  uint gpio_num = ROW[row_now];
  // 清除PWM中断
  pwm_clear_irq(pwm_gpio_to_slice_num(COL[0]));
  // 更新当前行号为下一个行号，循环遍历所有行
  row_now = (row_now + 1) & 7;
  // 遍历当前行的每个列，设置对应的GPIO电平来显示图像数据
  for (int i = 0; i < 8; i++)
  {
    pwm_set_gpio_level(COL[i], display_buffer[row_now][i]);
  }
  // 将选中的行GPIO设置为高电平
  gpio_put(gpio_num, 1); 
}
static void led_init(void)
{
  for (int i = 0; i < 8; i++)
  {
    gpio_init(COL[i]);
    gpio_init(ROW[i]);
    gpio_set_dir(COL[i], GPIO_OUT);
    gpio_set_dir(ROW[i], GPIO_OUT);
  }
  uint32_t slice_num_mask = 0;
  for (int i = 0; i < 8; i++)
  {
    // Tell the LED pin that the PWM is in charge of its value.
    gpio_set_function(COL[i], GPIO_FUNC_PWM);
    // Figure out which slice we just connected to the LED pin
    uint slice_num = pwm_gpio_to_slice_num(COL[i]);
    slice_num_mask |= (1 << slice_num);
    // Mask our slice's IRQ output into the PWM block's single interrupt line,
    // and register our interrupt handler
    if (0 == i)
    {
      pwm_clear_irq(slice_num);
      pwm_set_irq_enabled(slice_num, true);
      irq_set_exclusive_handler(PWM_IRQ_WRAP, on_pwm_wrap);
      irq_set_priority(PWM_IRQ_WRAP, 0);
      irq_set_enabled(PWM_IRQ_WRAP, true);
    }

    pwm_config c = {0, 0, 0};
    pwm_config_set_phase_correct(&c, true);
    pwm_config_set_clkdiv_int(&c, 1);
    pwm_config_set_clkdiv_mode(&c, PWM_DIV_FREE_RUNNING);
    pwm_config_set_output_polarity(&c, true, true);
    pwm_config_set_wrap(&c, LED_MAX - 1);
    pwm_init(slice_num, &c, false);
  }
  pwm_set_mask_enabled(slice_num_mask);
}
static void update_diaplay_buffer(uint64_t data, int max_brightness)
{
  uint64_t mask = 1;
  for(int row=0;row<8;row++){
    for(int col=7;col>=0;col--){
      uint16_t temp = (data & mask) ? max_brightness : 0;
      display_buffer[col][row] = LED_MAX - temp;
      mask <<= 1;
    }
  }
}

/* LSM6DS3TR-C 控制代码 -------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/

#define BOOT_TIME 15       // ms
#define WAIT_TIME_A 100    // ms
#define WAIT_TIME_G_01 150 // ms
#define WAIT_TIME_G_02 50  // ms

/* Self test limits. */
#define MIN_ST_LIMIT_mg 90.0f
#define MAX_ST_LIMIT_mg 1700.0f
#define MIN_ST_LIMIT_mdps 150000.0f
#define MAX_ST_LIMIT_mdps 700000.0f

/* Self test results. */
#define ST_PASS 1U
#define ST_FAIL 0U

/* Private variables ---------------------------------------------------------*/

/* Extern variables ----------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/

/*
 *   WARNING:
 *   Functions declare in this section are defined at the end of this file
 *   and are strictly related to the hardware platform used.
 *
 */
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,
                              uint16_t len);
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
                             uint16_t len);
static void platform_delay(uint32_t ms);
static void platform_init(void);

/* Main Example --------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#define BOOT_TIME 15 // ms
void lsm6ds3tr_c_read_data_polling(void)
{
  static int16_t data_raw_angular_rate[3];
  static int16_t data_raw_temperature;
  static float angular_rate_mdps[3];
  static float temperature_degC;
  static uint8_t whoamI, rst;
  /* Initialize mems driver interface */
  stmdev_ctx_t dev_ctx;
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = 0;
  /* Init test platform */
  platform_init();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  whoamI = 0;
  lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);
  printf("device_id = %x\n", whoamI);
  if (whoamI != LSM6DS3TR_C_ID)
    while (1)
      ; /*manage here device not found */

  /* Restore default configuration */
  lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do
  {
    lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_12Hz5);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_12Hz5);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_2g);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
  /* Configure filtering chain(No aux interface) */
  /* Accelerometer - analog filter */
  // lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx,
  //                                  LSM6DS3TR_C_XL_ANA_BW_400Hz);
  /* Accelerometer - LPF1 path ( LPF2 not used )*/
  // lsm6ds3tr_c_xl_lp1_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4);
  /* Accelerometer - LPF1 + LPF2 path */
  // lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx,
  //                                  LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100);
  /* Accelerometer - High Pass / Slope path */
  // lsm6ds3tr_c_xl_reference_mode_set(&dev_ctx, PROPERTY_DISABLE);
  // lsm6ds3tr_c_xl_hp_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_HP_ODR_DIV_100);
  /* Gyroscope - filtering chain */
  lsm6ds3tr_c_gy_band_pass_set(&dev_ctx,
                               LSM6DS3TR_C_HP_260mHz_LP1_STRONG);

  /* Read samples in polling mode (no int) */
  while (1)
  {
    /* Read output only if new value is available */
    lsm6ds3tr_c_reg_t reg;
    lsm6ds3tr_c_status_reg_get(&dev_ctx, &reg.status_reg);
    if (reg.status_reg.xlda)
    {
      /* Read magnetic field data */
      int16_t data_raw_acceleration[3] = {0};
      lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
      // lsm6ds3tr_c_from_fs2g_to_mg
      float ax = data_raw_acceleration[0];
      float ay = data_raw_acceleration[1];
      float az = data_raw_acceleration[2];
      float angx = atan2f(ax, -az) * 180.0f / M_PIf;
      float angy = atan2f(ay, -az) * 180.0f / M_PIf;
      printf("%.1f, %.1f\n", angx, angy);
    }
#if 0
    if (reg.status_reg.gda) {
      /* Read magnetic field data */
      memset(data_raw_angular_rate, 0, 3 * sizeof(int16_t));
      lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx,
                                       data_raw_angular_rate);
      angular_rate_mdps[0] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
                               data_raw_angular_rate[0]);
      angular_rate_mdps[1] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
                               data_raw_angular_rate[1]);
      angular_rate_mdps[2] = lsm6ds3tr_c_from_fs2000dps_to_mdps(
                               data_raw_angular_rate[2]);
      printf("%4.2f,%4.2f,%4.2f\n",
              angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
    }
#endif
#if 0
    if (reg.status_reg.tda) {
      /* Read temperature data */
      memset(&data_raw_temperature, 0, sizeof(int16_t));
      lsm6ds3tr_c_temperature_raw_get(&dev_ctx, &data_raw_temperature);
      temperature_degC = lsm6ds3tr_c_from_lsb_to_celsius(
                           data_raw_temperature );
      printf("Temperature [degC]:%6.2f\n",
              temperature_degC );
    }
#endif
  }
}
/*
 * @brief  Write generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to write
 * @param  bufp      pointer to data to write in register reg
 * @param  len       number of consecutive register to write
 *
 */
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,
                              uint16_t len)
{
  gpio_put(PIN_CS, 0);
  spi_write_blocking(SPI_PORT, &reg, 1);
  spi_write_blocking(SPI_PORT, bufp, len);
  gpio_put(PIN_CS, 1);
  return 0;
}

/*
 * @brief  Read generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to read
 * @param  bufp      pointer to buffer that store the data read
 * @param  len       number of consecutive register to read
 *
 */
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
                             uint16_t len)
{
  reg |= 0x80;
  gpio_put(PIN_CS, 0);
  spi_write_blocking(SPI_PORT, &reg, 1);
  spi_read_blocking(SPI_PORT, 0, bufp, len);
  gpio_put(PIN_CS, 1);
  return 0;
}

/*
 * @brief  platform specific delay (platform dependent)
 *
 * @param  ms        delay in ms
 *
 */
static void platform_delay(uint32_t ms)
{
  sleep_ms(ms);
}

/*
 * @brief  platform specific initialization (platform dependent)
 */
static void platform_init(void)
{
  // SPI initialisation. This example will use SPI at 1MHz.
  spi_init(SPI_PORT, 1000 * 1000);
  spi_set_format(SPI_PORT, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
  gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
  gpio_set_function(PIN_CS, GPIO_FUNC_SIO);
  gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
  gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);

  // Chip select is active-low, so we'll initialise it to a driven-high state
  gpio_set_dir(PIN_CS, GPIO_OUT);
  gpio_put(PIN_CS, 1);
}

int main()
{
  stdio_init_all();
  gpio_init(PWR_EN);
  gpio_set_dir(PWR_EN, GPIO_OUT);

  gpio_init(BUZ);
  gpio_set_dir(BUZ, GPIO_OUT);

  gpio_init(PWR_KEY);
  gpio_pull_up(PWR_KEY);

  gpio_init(P5V_SENSE);
  gpio_disable_pulls(P5V_SENSE);

  gpio_init(VBAT_EN);
  gpio_set_dir(VBAT_EN, GPIO_OUT);
  gpio_put(VBAT_EN, 1);

  adc_init();
  // Make sure GPIO is high-impedance, no pullups etc
  adc_gpio_init(VBAT_SENSE);
  // Select ADC input 0 (GPIO26)
  adc_select_input(VBAT_SENSE_ADC_CH);

  for (int i = 0; i < 1000; i++)
  {
    gpio_put(BUZ, 1);
    sleep_us(200);
    gpio_put(BUZ, 0);
    sleep_us(200);
  }
  gpio_put(PWR_EN, 1);
  update_diaplay_buffer(DIGIT0_9[0], 1000);
  led_init();
  // sleep_ms(3000);
  printf("Hello World!\n");
  // lsm6ds3tr_c_self_test();
  // lsm6ds3tr_c_read_data_polling();

  int i=0;
  while (1)
  {
    // gpio_put(COL[0], 1);
    if (!gpio_get(PWR_KEY))
    {
      sleep_ms(1000);
      if (!gpio_get(PWR_KEY))
      {
        // 在链接USB的状态下，长按电源按键进入固件更新模式
        if (gpio_get(P5V_SENSE))
        {
          // 有个BUG，进入bootloader模式后会断电
          // 所以需要一直按住电源按键不放
          reset_usb_boot(0, 0);
        }
        else
        {
          gpio_put(PWR_EN, 0);
        }
      }
    }
    // 12-bit conversion, assume max value == ADC_VREF == 3.3 V
    // const float conversion_factor = 6.0f * 3.3f / (1 << 12);
    // uint16_t result = adc_read();
    // printf("Raw value: 0x%03x, voltage: %f V\n", result, result * conversion_factor);
    sleep_ms(1);
    update_diaplay_buffer(DIGIT0_9[i/1000],i%1000);
    i = (i+1)%10000;
  }
  return 0;
}
